#include "ms618_algbackss1toss6circle.h"

using namespace MeasurE618;
AlgBackSS1toSS6Circle::AlgBackSS1toSS6Circle(int i):AlgBase(i)
{

}

void AlgBackSS1toSS6Circle::operator()(cv::Mat &srcImg,
                                       int id,
                                       std::map<string, float> &measRes,
                                       int fixtureId)
{
    std::vector<std::vector<cv::Point>> resptVec;
    resptVec.resize(7);
    for(auto i=20;i<26;i++)
    {
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-20,j));
        }
        //paras.push_back(tparas);
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point> contours;
        edgedetect obj;
        obj.NewCircleEdge(imgt,tparas,contours);

        ImageCoordinateTransformation(contours,rangVec);

        std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
        while (!AlgBase::flagVec[id])
        {
            (*AlgBase::cvarptrVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//            CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
            CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
            resptVec[i-20].push_back(ptsOut);
        }
    }
    {
        int i=20;
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-14,j));
        }
        //paras.push_back(tparas);
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point> contours;
        edgedetect obj;
        obj.GetLineContours(imgt,tparas,contours);

        ImageCoordinateTransformation(contours,rangVec);

        std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
        while (!AlgBase::flagVec[id])
        {
            (*AlgBase::cvarptrVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//            CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
            CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
            resptVec[i-14].push_back(ptsOut);
        }
    }


    // go on coding
    //cout<<"OK"<<endl;
    edgedetect obj;
    vector<vector<float>> cirres;
    for(int i=0;i<resptVec.size()-1;++i){
        cirres.push_back(obj.FitCircle(resptVec[i]));
    }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    measRes.insert(std::make_pair(string("FAI22-SS1-X-distance_pixel"), cirres[0][0]));
    float fai22ss1x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-SS1-X-distance")], cirres[0][0]);
    measRes.insert(std::make_pair(string("FAI22-SS1-X-distance_real"), fai22ss1x));

    measRes.insert(std::make_pair(string("FAI22-SS1-Y-distance_pixel"), -cirres[0][1]));
    float fai22ss1y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-SS1-Y-distance")], -cirres[0][1]);
    measRes.insert(std::make_pair(string("FAI22-SS1-Y-distance_real"), fai22ss1y));


    CalcPosition(prefix + "FAI22-SS1-X-distance", prefix + "FAI22-SS1-Y-distance", "FAI22-SS1-position", prefix, obj, m_standardValMap["FAI22-SS1-X-distance"], m_standardValMap["FAI22-SS1-Y-distance"],
                 cirres[0][0], -cirres[0][1], measRes);

    measRes.insert(std::make_pair(string("FAI22-SS2-X-distance_pixel"), cirres[1][0]));
    float fai22ss2x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-SS2-X-distance")], cirres[1][0]);
    measRes.insert(std::make_pair(string("FAI22-SS2-X-distance_real"), fai22ss2x));

    measRes.insert(std::make_pair(string("FAI22-SS2-Y-distance_pixel"), -cirres[1][1]));
    float fai22ss2y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-SS2-Y-distance")], -cirres[1][1]);
    measRes.insert(std::make_pair(string("FAI22-SS2-Y-distance_real"), fai22ss2y));

    CalcPosition(prefix + "FAI22-SS2-X-distance", prefix + "FAI22-SS2-Y-distance", "FAI22-SS2-position", prefix,
                 obj, m_standardValMap["FAI22-SS2-X-distance"], m_standardValMap["FAI22-SS2-Y-distance"],
                 cirres[1][0], -cirres[1][1], measRes);


    measRes.insert(std::make_pair(string("FAI22-SS3-X-distance_pixel"), cirres[2][0]));
    float fai22ss3x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-SS3-X-distance")], cirres[2][0]);
    measRes.insert(std::make_pair(string("FAI22-SS3-X-distance_real"), fai22ss3x));

    measRes.insert(std::make_pair(string("FAI22-SS3-Y-distance_pixel"), -cirres[2][1]));
    float fai22ss3y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-SS3-Y-distance")], -cirres[2][1]);
    measRes.insert(std::make_pair(string("FAI22-SS3-Y-distance_real"), fai22ss3y));

    CalcPosition(prefix + "FAI22-SS3-X-distance", prefix + "FAI22-SS3-Y-distance", "FAI22-SS3-position", prefix,
                 obj, m_standardValMap["FAI22-SS3-X-distance"],
                      m_standardValMap["FAI22-SS3-Y-distance"],
                      cirres[2][0], -cirres[2][1], measRes);

    measRes.insert(std::make_pair(string("FAI22-SS4-X-distance_pixel"), cirres[3][0]));
    float fai22ss4x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-SS4-X-distance")], cirres[3][0]);
    measRes.insert(std::make_pair(string("FAI22-SS4-X-distance_real"), fai22ss4x));

    measRes.insert(std::make_pair(string("FAI22-SS4-Y-distance_pixel"), -cirres[3][1]));
    float fai22ss4y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-SS4-Y-distance")], -cirres[3][1]);
    measRes.insert(std::make_pair(string("FAI22-SS4-Y-distance_real"), fai22ss4y));

    CalcPosition(prefix + "FAI22-SS4-X-distance", prefix + "FAI22-SS4-Y-distance", "FAI22-SS4-position", prefix,
                 obj, m_standardValMap["FAI22-SS4-X-distance"], m_standardValMap["FAI22-SS4-Y-distance"],
            cirres[3][0], -cirres[3][1], measRes);

    measRes.insert(std::make_pair(string("FAI22-SS5-X-distance_pixel"), cirres[4][0]));
    float fai22ss5x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-SS5-X-distance")], cirres[4][0]);
    measRes.insert(std::make_pair(string("FAI22-SS5-X-distance_real"), fai22ss5x));

    measRes.insert(std::make_pair(string("FAI22-SS5-Y-distance_pixel"), -cirres[4][1]));
    float fai22ss5y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-SS5-Y-distance")], -cirres[4][1]);
    measRes.insert(std::make_pair(string("FAI22-SS5-Y-distance_real"), fai22ss5y));

    CalcPosition(prefix + "FAI22-SS5-X-distance", prefix + "FAI22-SS5-Y-distance", "FAI22-SS5-position", prefix,
                 obj, m_standardValMap["FAI22-SS5-X-distance"], m_standardValMap["FAI22-SS5-Y-distance"],
            cirres[4][0], -cirres[4][1], measRes);

    measRes.insert(std::make_pair(string("FAI22-SS6-X-distance_pixel"), cirres[5][0]));
    float fai22ss6x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-SS6-X-distance")], cirres[5][0]);
    measRes.insert(std::make_pair(string("FAI22-SS6-X-distance_real"), fai22ss6x));

    measRes.insert(std::make_pair(string("FAI22-SS6-Y-distance_pixel"), -cirres[5][1]));
    float fai22ss6y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI22-SS6-Y-distance")], -cirres[5][1]);
    measRes.insert(std::make_pair(string("FAI22-SS6-Y-distance_real"), fai22ss6y));

    CalcPosition(prefix + "FAI22-SS6-X-distance", prefix + "FAI22-SS6-Y-distance", "FAI22-SS6-position", prefix,
                 obj, m_standardValMap["FAI22-SS6-X-distance"], m_standardValMap["FAI22-SS6-Y-distance"],
            cirres[5][0], -cirres[5][1], measRes);

    measRes.insert(std::make_pair(string("FAI91-SS1-diam_pixel"),cirres[0][2]));
    float fai91ss1d = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI91-SS1-diam")], cirres[0][2]);
    measRes.insert(std::make_pair(string("FAI91-SS1-diam_real"), fai91ss1d));

    measRes.insert(std::make_pair(string("FAI91-SS2-diam_pixel"),cirres[1][2]));
    float fai91ss2d = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI91-SS2-diam")], cirres[1][2]);
    measRes.insert(std::make_pair(string("FAI91-SS2-diam_real"), fai91ss2d));

    measRes.insert(std::make_pair(string("FAI91-SS3-diam_pixel"),cirres[2][2]));
    float fai91ss3d = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI91-SS3-diam")], cirres[2][2]);
    measRes.insert(std::make_pair(string("FAI91-SS3-diam_real"), fai91ss3d));

    measRes.insert(std::make_pair(string("FAI91-SS4-diam_pixel"),cirres[3][2]));
    float fai91ss4d = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI91-SS4-diam")], cirres[3][2]);
    measRes.insert(std::make_pair(string("FAI91-SS4-diam_real"), fai91ss4d));

    measRes.insert(std::make_pair(string("FAI91-SS5-diam_pixel"),cirres[4][2]));
    float fai91ss5d = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI91-SS5-diam")], cirres[4][2]);
    measRes.insert(std::make_pair(string("FAI91-SS5-diam_real"), fai91ss5d));

    measRes.insert(std::make_pair(string("FAI91-SS6-diam_pixel"),cirres[5][2]));
    float fai91ss6d = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI91-SS6-diam")], cirres[5][2]);
    measRes.insert(std::make_pair(string("FAI91-SS6-diam_real"), fai91ss6d));

    float fai11= 0;
    for(int i = 0; i < resptVec[6].size(); ++i){
        if(fai11 < abs(resptVec[6][i].x)){
            fai11 = abs(resptVec[6][i].x);
        }
    }
    measRes.insert(std::make_pair(string("FAI11-SS1-X-distance_pixel"), fai11));
    float fai11ss1x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI11-SS1-X-distance")], fai11);
    measRes.insert(std::make_pair(string("FAI11-SS1-X-distance_real"), fai11ss1x));

    qDebug() << "E618 alg back ss1 to ss6 circle end";
}
